#include "ME.h"
#include <algorithm>
using namespace std;

ME::ME(void){
}

ME::~ME(void){
}

// --------------------------------------------------------------------------
#define RESET_ALLOWED_DIRECTIONS(pArray) {\
	memset(pArray, TRUE, 9);\
	pArray[4] = FALSE;\
}

int ME::doDomainMotionEstimationFull(byte *pBFrame, byte *pDomain, int iXpos, int iYpos, int &mx, int &my, int iSearchDistance, int iDomainWidth, int iDomainHeight, int iFrameWidth, int iFrameHeight){
	int _x1, _x2, _y1, _y2,i,j;
	_x1 = ((iXpos - iSearchDistance) < 0)?0:(iXpos - iSearchDistance);	//Left X allowed position
	_y1 = ((iYpos - iSearchDistance) < 0)?0:(iYpos - iSearchDistance);	//Top Y allowed position
	_x2 = min(iXpos + iSearchDistance, iFrameWidth - iDomainWidth-1);	//Right X allowed position
	_y2 = min(iYpos + iSearchDistance, iFrameHeight - iDomainHeight-1);	//Bottom Y allowed position*/

	int bestDiff = getL2DomainDistance(pBFrame, pDomain, iXpos, iYpos, iDomainWidth, iDomainHeight, iFrameWidth, iFrameHeight); //getL2 at center
	mx=0;
	my=0;
	for(i=_y1; i<_y2; i++){
		for(j=_x1; j<_x2; j++){
			int diff = getL2DomainDistance(pBFrame, pDomain, j, i, iDomainWidth, iDomainHeight, iFrameWidth, iFrameHeight);
			if (diff < bestDiff) {
					mx = j-iXpos;
					my = i-iYpos;
					bestDiff = diff;
			}
		}
	}
	return bestDiff;
}

int ME::doDomainMotionEstimation3SS(byte *pBFrame, byte *pDomain, int iXpos, int iYpos, int &mx, int &my, int iSearchDistance, int iDomainWidth, int iDomainHeight, int iFrameWidth, int iFrameHeight){
	

	byte pAllowedDirections[9];

	static const int pFactors[9][2] = { {-1,-1}, { 0,-1}, {1, -1},
										{-1, 0}, { 0, 0}, {+1, 0},
										{-1, 1}, { 0,+1}, {+1, +1}};

	bool bFoundBetterThreshhold = true;
	
	int bestDiff = getL2DomainDistance(pBFrame, pDomain, iXpos, iYpos, iDomainWidth, iDomainHeight, iFrameWidth, iFrameHeight); //getL2 at center
	mx = 0; my = 0;

	int iBestXpos;
    int iBestYpos;

	int iNewXpos = iXpos;
	int iNewYpos = iYpos;
	while (iSearchDistance > 0) {
		RESET_ALLOWED_DIRECTIONS(pAllowedDirections);
		//checking for allowed directions 
		if (iNewXpos - iSearchDistance < 0) {
			pAllowedDirections[0] = FALSE;
			pAllowedDirections[3] = FALSE;
			pAllowedDirections[6] = FALSE;
		}

		if (iNewXpos + iDomainWidth + iSearchDistance >= iFrameWidth) {
			pAllowedDirections[2] = FALSE;
			pAllowedDirections[5] = FALSE;
			pAllowedDirections[8] = FALSE;
		}

		if (iNewYpos - iSearchDistance < 0) {
			pAllowedDirections[0] = FALSE;
			pAllowedDirections[1] = FALSE;
			pAllowedDirections[2] = FALSE;
		}

		if (iNewYpos + iDomainHeight + iSearchDistance >= iFrameHeight) {
			pAllowedDirections[6] = FALSE;
			pAllowedDirections[7] = FALSE;
			pAllowedDirections[8] = FALSE;
		}

		int iBestXpos = iNewXpos;
		int iBestYpos = iNewYpos;

		for (int i = 0 ; i < 9; i++) {
			if (pAllowedDirections[i]) {
				int iX = (iNewXpos + (pFactors[i][0] * iSearchDistance));
				int iY = (iNewYpos + (pFactors[i][1] * iSearchDistance));
				int diff = getL2DomainDistance(pBFrame, pDomain, iX, iY, iDomainWidth, iDomainHeight, iFrameWidth, iFrameHeight);
				if (diff < bestDiff) {
					iBestXpos = iX;
					iBestYpos = iY;
					bestDiff = diff;
				}
			}
		}

		iNewXpos = iBestXpos;
		iNewYpos = iBestYpos;

		iSearchDistance /= 2;
	}

	mx = iNewXpos - iXpos;
	my = iNewYpos - iYpos;
	return bestDiff;
}

int ME::getL2DomainDistance(byte *pBFrame, 
							byte *pDomain, 
							unsigned uXPos, 
							unsigned uYPos, 
							unsigned uDomainWidth, 
							unsigned uDomainHeight,
							unsigned uFrameWidth, 
							unsigned uFrameHeight){
	int dist = 0;
	for (unsigned i = 0 ; i < uDomainHeight;i++) 
		for (unsigned j = 0; j < uDomainWidth;j++)
			dist += (pBFrame[uFrameWidth * (uYPos + i) + uXPos + j] - pDomain[i * uDomainWidth + j]) * 
					(pBFrame[uFrameWidth * (uYPos + i) + uXPos + j] - pDomain[i * uDomainWidth + j]);

	//return ((int)  sqrt(dist));
	//reduce sqrt for speed
	return dist;
}